# coding: utf8

import time
import threading

from engine import Engine
from navigator import Navigator

engine = Engine(in1=11, in2=12, in3=13, in4=15)
navigator = Navigator(in1=16, in2=18, in3=19)


def loop():
    while True:
        if engine.stopped:
            continue
        dis = navigator.check_front()
        while dis < 0.35:
            engine.stop()
            l_dis = navigator.check_left()
            r_dis = navigator.check_right()
            if l_dis > 0.35 or r_dis > 0.35:
                if l_dis > r_dis:
                    engine.turn_left()
                else:
                    engine.turn_right()
                time.sleep(2)
                engine.forward()
                navigator.forward()
                dis = max([l_dis, r_dis])
            else:
                engine.back()
                time.sleep(2)

        time.sleep(0.1)


t1 = threading.Thread(target=loop)
# t1.start()

while True:
    cmd = input("Please type a command to control the car: ")
    if cmd == 'f':
        engine.forward()
    elif cmd == 'b':
        engine.back()
    elif cmd == 'l':
        engine.turn_left()
    elif cmd == 'r':
        engine.turn_right()
    elif cmd == 's':
        engine.stop()
    elif cmd == 'e':
        engine.stop()
    elif cmd == 't':
        navigator.turn_left()
    elif cmd == 'q':
        navigator.turn_right()
    else:
        navigator.setDuty(float(cmd))